#include "TorTargetAcquire.h"  



#define Y_IMAGE_RES 480         //X Image resolution in pixels, should be 120, 240 or 480
//#define VIEW_ANGLE 49           //Axis M1013
//#define VIEW_ANGLE 41.7               //Axis 206 camera
#define VIEW_ANGLE 37.4  //Axis M1011 camera
#define PI 3.141592653

//Score limits used for target identification
#define RECTANGULARITY_LIMIT 40
#define ASPECT_RATIO_LIMIT 55

//Score limits used for hot target determination
#define TAPE_WIDTH_LIMIT 50
#define VERTICAL_SCORE_LIMIT 50
#define LR_SCORE_LIMIT 50

//Minimum area of particles to be considered
#define AREA_MINIMUM 150

//Maximum number of particles to process
#define MAX_PARTICLES 8


TorTargetAcquire::TorTargetAcquire()
: m_camera ( AxisCamera::GetInstance())
{

}

double TorTargetAcquire::computeDistance (BinaryImage *image, ParticleAnalysisReport *report) {
  double rectLong, height;
  int targetHeight;

  imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
  //using the smaller of the estimated rectangle long side and the bounding rectangle height results in better performance
  //on skewed rectangles
  height = min(report->boundingRect.height, rectLong);
  targetHeight = 32;

  return Y_IMAGE_RES * targetHeight / (height * 12 * 2 * tan(VIEW_ANGLE*PI/(180*2)));
}

/**
 * Computes a score (0-100) comparing the aspect ratio to the ideal aspect ratio for the target. This method uses
 * the equivalent rectangle sides to determine aspect ratio as it performs better as the target gets skewed by moving
 * to the left or right. The equivalent rectangle is the rectangle with sides x and y where particle area= x*y
 * and particle perimeter= 2x+2y
 * 
 * @param image The image containing the particle to score, needed to perform additional measurements
 * @param report The Particle Analysis Report for the particle, used for the width, height, and particle number
 * @param outer Indicates whether the particle aspect ratio should be compared to the ratio for the inner target or the outer
 * @return The aspect ratio score (0-100)
 */
double TorTargetAcquire::scoreAspectRatio(BinaryImage *image, ParticleAnalysisReport *report, bool vertical){
  double rectLong, rectShort, idealAspectRatio, aspectRatio;
  idealAspectRatio = vertical ? (4.0/32) : (23.5/4);      //Vertical reflector 4" wide x 32" tall, horizontal 23.5" wide x 4" tall

  imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
  imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);

  //Divide width by height to measure aspect ratio
  if(report->boundingRect.width > report->boundingRect.height){
      //particle is wider than it is tall, divide long by short
      aspectRatio = ratioToScore(((rectLong/rectShort)/idealAspectRatio));
  } else {
      //particle is taller than it is wide, divide short by long
      aspectRatio = ratioToScore(((rectShort/rectLong)/idealAspectRatio));
  }
  return aspectRatio;             //force to be in range 0-100
}

/**
 * Compares scores to defined limits and returns true if the particle appears to be a target
 * 
 * @param scores The structure containing the scores to compare
 * @param vertical True if the particle should be treated as a vertical target, false to treat it as a horizontal target
 * 
 * @return True if the particle meets all limits, false otherwise
 */
bool TorTargetAcquire::scoreCompare(Scores scores, bool vertical){
  bool isTarget = true;

  isTarget &= scores.rectangularity > RECTANGULARITY_LIMIT;
  if(vertical){
      isTarget &= scores.aspectRatioVertical > ASPECT_RATIO_LIMIT;
  } else {
      isTarget &= scores.aspectRatioHorizontal > ASPECT_RATIO_LIMIT;
  }

  return isTarget;
}

/**
 * Computes a score (0-100) estimating how rectangular the particle is by comparing the area of the particle
 * to the area of the bounding box surrounding it. A perfect rectangle would cover the entire bounding box.
 * 
 * @param report The Particle Analysis Report for the particle to score
 * @return The rectangularity score (0-100)
 */
double TorTargetAcquire::scoreRectangularity(ParticleAnalysisReport *report){
  if(report->boundingRect.width*report->boundingRect.height !=0){
      return 100*report->particleArea/(report->boundingRect.width*report->boundingRect.height);
  } else {
      return 0;
  }       
}       

/**
 * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise
 * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2
 */
double TorTargetAcquire::ratioToScore(double ratio)
{
  return (max(0, min(100*(1-fabs(1-ratio)), 100)));
}

/**
 * Takes in a report on a target and compares the scores to the defined score limits to evaluate
 * if the target is a hot target or not.
 * 
 * Returns True if the target is hot. False if it is not.
 */
bool TorTargetAcquire::hotOrNot(TargetReport target)
{
  bool isHot = true;

  isHot &= target.tapeWidthScore >= TAPE_WIDTH_LIMIT;
  isHot &= target.verticalScore >= VERTICAL_SCORE_LIMIT;
  isHot &= (target.leftScore > LR_SCORE_LIMIT) | (target.rightScore > LR_SCORE_LIMIT);

  return isHot;
}
ColorImage* TorTargetAcquire::ImageAcquire()
{
  
}

void TorTargetAcquire::visionTracking()
{


  if(m_camera.IsFreshImage()) 
    {
      ds->Printf(DriverStationLCD::kUser_Line1, 1, "Fresh Image Detected");
      ds->UpdateLCD();
      int counter = 0;
      Scores *scores;
      TargetReport target;
      int verticalTargets[MAX_PARTICLES];
      int horizontalTargets[MAX_PARTICLES];
      int verticalTargetCount, horizontalTargetCount;
      Threshold threshold (150, 255, 230, 255, 230, 255); //ORIGINAL VALS(105, 137, 230, 255, 133, 183); 60,150, 90, 255,20,255      //HSV threshold criteria, ranges are in that order ie. Hue is 60-100
      Threshold testThreshold (0, 359, 0, 255, 0, 255);
      ParticleFilterCriteria2 criteria[] = {
          {IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
      };                                   

      //    AxisCamera &camera = AxisCamera::GetInstance();
      //        camera.WriteResolution(AxisCamera::kResolution_320x240);
      //        camera.WriteBrightness(50);
      //        camera.WriteCompression(20);
      ColorImage *image;
      //image = new RGBImage("/testImage.jpg");         // get the sample image from the cRIO flash
      ds->Clear();
      image = m_camera.GetImage();                            //To get the images from the camera comment the line above and uncomment this one
      image->Write("/raw.bmp");

      while (counter < 5)
        {

          ds->Clear();
          BinaryImage *thresholdImage = image->ThresholdRGB(threshold);   // get just the green target pixels
          thresholdImage->Write("/threshold.bmp");        
          //test image
          //BinaryImage *rawImage = image->ThresholdRGB(0,255,0,255,0,255);
          //rawImage->Write("/raw.bmp");
          BinaryImage *filledImage = thresholdImage->ConvexHull(false); //fill in particles
          filledImage->Write("/filled.bmp");

          ds->Printf(DriverStationLCD::kUser_Line2, 2, "Image 1 Printed");
          ds->UpdateLCD();
          BinaryImage *filteredImage = filledImage->ParticleFilter(criteria, 1);       //Remove small particles
          filteredImage->Write("/Filtered.bmp");
          ds->Printf(DriverStationLCD::kUser_Line3, 2, "Image 2 Printed");
          ds->UpdateLCD();

          vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports(); 

          verticalTargetCount = horizontalTargetCount = 0;

          if(reports->size() > 0)
            {
              scores = new Scores[reports->size()];
              for (unsigned int i = 0; i < MAX_PARTICLES && i < reports->size(); i++)
                {
                  ParticleAnalysisReport *report = &(reports->at(i));
                  //TODO PRINT REPORTS AND CHECK THE PARTICLES

                  scores[i].rectangularity = scoreRectangularity(report);
                  scores[i].aspectRatioVertical = scoreAspectRatio(filteredImage, report, true);
                  scores[i].aspectRatioHorizontal = scoreAspectRatio(filteredImage, report, false);
                  if(scoreCompare(scores[i], false))
                    {
                      //printf("particle: %d  is a Horizontal Target centerX: %d  centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
                      ds->Printf(DriverStationLCD::kUser_Line4, 1, "Horiz(x): %d", report->center_mass_x);
                      ds->Printf(DriverStationLCD::kUser_Line5,1, "Horiz(y): %d", report->center_mass_y);
                      counter = 50;
                      break;
                      //horizontalTargets[horizontalTargetCount++] = i; //Add particle to target array and increment count
                    } 
                  else if (scoreCompare(scores[i], true)) 
                    { 
                      ds->Printf(DriverStationLCD::kUser_Line4, 1, "Vert(x): %d",report->center_mass_x);
                      ds->Printf(DriverStationLCD::kUser_Line5,1, "Vert(y): %d",report->center_mass_y);
                      counter = 50;
                      break;
                      //printf("particle: %d  is a Vertical Target centerX: %d  centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
                      //verticalTargets[verticalTargetCount++] = i;  //Add particle to target array and increment count
                    } 
                  else 
                    {
                      ds->Printf(DriverStationLCD::kUser_Line4, 1, "Not Target");
                      ds->Printf(DriverStationLCD::kUser_Line5,1, "Not Target");
                      counter++;
                      //printf("particle: %d  is not a Target centerX: %d  centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
                    }
                }
            }
          else
            {
              counter++;
            }

          delete filteredImage;
          delete thresholdImage;

          delete scores;
          delete reports;
          ds->Printf(DriverStationLCD::kUser_Line6, 2, "Image Deleted");
          ds->UpdateLCD();
        }
      delete image;
    } 
  else 
    {
      ds->Printf(DriverStationLCD::kUser_Line1, 1, "No Fresh Image Detected");
      ds->UpdateLCD();
    }


}
